#!/usr/bin/env roseus
(ros::load-ros-manifest "ar_pose")

;; <parameter>
;; *robot2dpos* screenpos of robot
;; *robot2dori* screenori of robot
;; *dest2dpos*  screenpos of where user clicked

(defun ar-cb (msg)
  (let* ((cds (ros::tf-pose->coords (send msg :pose :pose)))
	 (z-depth (send msg :pose :pose :position :z))
	 (gain (* 1000 z-depth)))
    (setq *robot2dpos*
	  (float-vector (* (send msg :pose :pose :position :x) gain)
			(* (send msg :pose :pose :position :y) gain)))
    ;; extracting angle, assumed that robot is in a plane
    (setq *robot2dori*
	  (float-vector (cos (elt (car (send cds :rpy-angle)) 0))
			(sin (elt (car (send cds :rpy-angle)) 0))))
    (ros::ros-error "robot is in ~A ~A" *robot2dpos* *robot2dori*)
    (setq cc 0)
    ))

(defun sp-cb (msg)
  (setq *dest2dpos* (float-vector (- (send msg :point :x) (/ *width* 2))
				  (- (send msg :point :y) (/ *height* 2))))
  ;;(ros::ros-info "destination is in ~A" *dest2dpos*)
  )

(defun process ()
  (if (or (null *dest2dpos*)(null *robot2dpos*)) (return-from process nil))
  (let ((dis (distance *dest2dpos* *robot2dpos*))
	(angle (acos (v. (normalize-vector (v- *dest2dpos* *robot2dpos*)) *robot2dori*)))
	(msg (instance geometry_msgs::Twist :init)))
    (ros::ros-warn "angle:~A" angle)
    (ros::ros-warn "distance:~A" dis)
    (incf cc)
    (cond ((or (> 100 dis) (> cc 3))
	   (ros::ros-info "Stop")
	   (send msg :linear :x 0.0)
	   (send msg :angular :z 0.0)
	   (ros::publish "/cmd_vel" msg)
	   (unix::sleep 1)
	   )
	  ((< angle 0.2)
	   (ros::ros-info "Go straight")
	   (send msg :linear :x 0.1)
	   (send msg :angular :z 0)
	   (ros::publish "/cmd_vel" msg)
	   )
	  ((> angle 2.9)
	   (ros::ros-info "Go back")
	   (send msg :linear :x -0.1)
	   (send msg :angular :z 0)
	   (ros::publish "/cmd_vel" msg)
	   )
	  (t
	   (ros::ros-info "Rotate")
	   (send msg :angular :z 0.04)
	   (ros::publish "/cmd_vel" msg)
	   )
	  )
    (unix::usleep (* 1000 500))
    ))

(defun init-setting ()
  (ros::roseus "ros_matereal")
  (ros::advertise "/cmd_vel" geometry_msgs::Twist 1)
  (ros::subscribe "/ar_pose_marker" ar_pose::ARMarker #'ar-cb)
  (ros::subscribe "/usb_cam/image_raw/screenpoint" geometry_msgs::PointStamped #'sp-cb)
  (setq *robot2dpos* nil *robot2dpos* nil *dest2dpos* nil)
  ;; camera's width and height are assumed to be (640 480) now
  ;; if they change, please subscribe to /usb_cam/image_raw/{width,height}
  (setq *width* 640 *height* 480)
  )

(init-setting)
(do-until-key
 (ros::sleep)
 (ros::spin-once)
 (process)
 )